import math
import random
from Node import Node
from point import point
from plotmaze import *


class RRTstar:
    def __init__(self, obstacle, startnode, endnode, length, width, step, sigma=1):
        self.leaf = []
        self.path = []
        self.guasstemp = 0
        self.randomtemp = 0

        self.mazeObstacle = obstacle  # 地图信息，记录不可行点
        self.startNode = startnode
        self.endNode = endnode
        self.length = length
        self.width = width
        self.step = step
        self.sigma = sigma

        self.randnode = None
        self.currNode = None

    def minDisnode(self):  # 计算leaf节点中的最小距离的节点
        if len(self.leaf) == 0:
            plotmaze(self.mazeObstacle, (self.startNode.point.x, self.startNode.point.y),
                     (self.endNode.point.x, self.endNode.point.y), self.length, self.width)
            return None
        else:
            minNode = self.leaf[0]
            for tempNode in self.leaf:
                if tempNode.distance < minNode.distance:
                    minNode = tempNode
            return minNode

    def getRandnode(self):
        # print("try")
        if self.guasstemp < 5:
            flag = True
            while flag:
                tempx = random.gauss(self.endNode.point.x, self.sigma)
                tempy = random.gauss(self.endNode.point.y, self.sigma)
                if self.width - 1 >= tempx >= 0 and self.length - 1 >= tempy >= 0:
                    self.randnode = Node(point(tempx, tempy))
                    print(tempx, tempy)
                    flag = False
        elif 5 <= self.guasstemp < 10:
            flag = True
            while flag:
                ranint = random.randint(0,3)
                if ranint == 0 or ranint == 1:
                    tempx = random.gauss(self.endNode.point.x, self.sigma * 5)
                    tempy = random.gauss(self.endNode.point.y, self.sigma * 5)
                elif ranint == 2:
                    tempx = random.gauss(self.endNode.point.x, self.sigma * 2)
                    tempy = random.gauss(self.width - 1, self.sigma * 2)
                elif ranint == 3:
                    tempx = random.gauss(self.length - 1, self.sigma * 2)
                    tempy = random.gauss(self.endNode.point.y, self.sigma * 2)

                if self.width - 1 >= tempx >= 0 and self.length - 1 >= tempy >= 0:
                    self.randnode = Node(point(tempx, tempy))
                    print(tempx, tempy)
                    flag = False

            print("middle")
        elif self.randomtemp >= 10:
            flag = True
            while flag:
                tempx = random.gauss(self.currNode.point.x, self.sigma * 5)
                tempy = random.gauss(self.currNode.point.y, self.sigma * 5)
                if self.width - 1 >= tempx >= 0 and self.length - 1 >= tempy >= 0:
                    self.randnode = Node(point(tempx, tempy))
                    print(tempx, tempy)
                    flag = False
            print("bad")
        else:
            print("random")
            self.randnode = Node(point(random.uniform(0, self.length - 1), random.uniform(0, self.width - 1)))
            self.randomtemp = self.randomtemp + 1


    def getLeafdis(self):
        for tempNode in self.leaf:
            if self.randnode is not None:
                tempNode.getDis(self.randnode)

    def isAccess(self, thisPoint):  # 判断point是否可通行
        if thisPoint.x < 0 or thisPoint.y < 0:  # 低于下界
            return False
        if thisPoint.x >= self.length - 1 or thisPoint.y >= self.width - 1:  # 高于上界
            return False
        for i in self.mazeObstacle:  # 处于障碍物内
            if pow(i[0] - thisPoint.x, 2) + pow(i[1] - thisPoint.y, 2) <= pow(i[2], 2):
                return False
        return True

    def Heron(self, a, b, c):
        p = (a + b + c) / 2
        S = math.sqrt(p * (p - a) * (p - b) * (p - c))
        h = 2 * S / a
        return h

    def isSafe(self, newPoint, nearNode):
        nearNode.getDis(Node(newPoint))
        newNear = nearNode.distance
        for i in self.mazeObstacle:
            newDx = newPoint.x - i[0]
            newDy = newPoint.y - i[1]
            nearDx = nearNode.point.x - i[0]
            nearDy = nearNode.point.y - i[1]

            newO = math.sqrt(pow(newDx, 2) + pow(newDy, 2))
            nearO = math.sqrt(pow(nearDx, 2) + pow(nearDy, 2))

            h = self.Heron(newNear, newO, nearO) # 海伦公式
            cosnew = (pow(newNear, 2) + pow(newO, 2) - pow(nearO, 2)) / (2 * newO * newNear)
            cosnear = (pow(newNear, 2) + pow(nearO, 2) - pow(newO, 2)) / (2 * nearO * newNear) # 余弦定理

            if h <= i[2] and cosnew > 0 and cosnear > 0:
                return False
        return True

    def isLast(self, thisNode):
        thisNode.getDis(self.endNode)
        if thisNode.distance <= self.step:
            print('end')
            return True
        else:
            return False

    def findFather(self, newNode):
        self.randnode = newNode
        self.getLeafdis()
        nearNode = self.minDisnode()
        newNode.ancientNode(nearNode)
        print("findF")
        return newNode

    def recreateTree(self,newNode):
        minCost = newNode.ancient.cost + newNode.ancient.distance
        for node in self.leaf:
            tempCost = node.distance + node.cost
            if tempCost <= minCost:
                if self.isSafe(newNode.point, node):
                    minCost = tempCost
                    newNode.ancientNode(node)
        newNode.cost = minCost
        return newNode


    def newLeaf(self):
        if len(self.leaf) == 0:
            self.leaf.append(self.startNode)
            self.currNode = self.startNode
        else:
            self.getRandnode()  # 获取rand节点
            self.getLeafdis()  # 计算所有叶子节点到rand节点的距离
            nearNode = self.minDisnode()  # 找到距离rand节点距离最近的叶子节点
            randNode = self.randnode  # 将rand节点非self化
            # nearNode.getDis(randNode)  # 再计算一次距离rand节点距离，应该可删

            tempPoint = randNode.point

            if self.isAccess(tempPoint) and self.isSafe(tempPoint, nearNode):
                newPoint = point(tempPoint.x * 0.75 + nearNode.point.x * 0.25, tempPoint.y * 0.75 + nearNode.point.y * 0.25)
                self.guasstemp = 0
                self.randomtemp = 0
            elif self.isAccess(point((tempPoint.x + nearNode.point.x) / 2, (tempPoint.y + nearNode.point.y) / 2)) \
                    and self.isSafe(point((tempPoint.x + nearNode.point.x) / 2, (tempPoint.y + nearNode.point.y) / 2), nearNode):
                newPoint = point((tempPoint.x + nearNode.point.x) / 2, (tempPoint.y + nearNode.point.y) / 2)
                self.guasstemp = 0
                self.randomtemp = 0
            else:
                gapX = randNode.point.x - nearNode.point.x
                gapY = randNode.point.y - nearNode.point.y  # 两点坐标差
                gapD = math.sqrt(pow(gapX, 2) + pow(gapY, 2))  # 两点距离
                sin = gapX / gapD
                cos = gapY / gapD  # 两点角度

                newX = nearNode.point.x + sin * self.step
                newY = nearNode.point.y + cos * self.step
                thePoint = point(newX, newY)  # 按一定步长沿该方向找到新节点
                # print("new:",newX, newY)
                # print("near:",nearNode.point.x, nearNode.point.y)
                # print("sin:",sin," cos:",cos," gapX:",gapX," gapY:",gapY," gapD:",gapD)
                if self.isAccess(thePoint) and self.isSafe(thePoint, nearNode):  # 新节点是否可达
                    newPoint = thePoint
                    self.guasstemp = 0
                    self.randomtemp = 0
                else:
                    # print("?")
                    self.guasstemp = self.guasstemp + 1
                    return

            # if cos <= math.cos(self.maxangle / 180 * math.pi):  # 该路线角度是否过大
            newNode = Node(newPoint)
            newNode = self.findFather(newNode)
            newNode = self.recreateTree(newNode)
            self.leaf.append(newNode)
            self.currNode = newNode
            plotAnimation(self.mazeObstacle, self.leaf, newPoint, nearNode.point, self.startNode.point, self.endNode.point, self.length, self.width)

    def run(self):
        temp = 0
        fig = plt.figure(1)
        while True:
            temp = temp + 1
            self.newLeaf()
            #plt.show()
            plt.pause(2)
            plt.close('all')
            if self.isLast(self.currNode) is True:
                print("last")
                self.endNode.ancient = self.currNode
                self.endNode.point.x = self.endNode.point.x - 1
                self.endNode.point.y = self.endNode.point.y - 1
                tempNode = self.endNode
                while True:
                    self.path.append(tempNode)
                    if tempNode.ancient != None:
                        tempNode = tempNode.ancient
                    else:
                        return self.path
            elif len(self.leaf) >= self.length * self.width * 0.1:
                return False